package com.example.javaquest;

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Container;
import java.awt.Dimension;
import java.awt.FlowLayout;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.image.BufferedImage;
import java.awt.image.DataBufferUShort;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.text.DecimalFormat;

import javax.swing.ImageIcon;
import javax.swing.JFrame;
import javax.swing.JLabel;

import com.zonerobotics.zBotAPI.Enums.EzEvent;
import com.zonerobotics.zBotAPI.Enums.EzReadVars;
import com.zonerobotics.zBotAPI.ZBot.IBotListener;
import com.zonerobotics.zBotAPI.ZBot.IZBot;
import com.zonerobotics.zBotAPI.ZBot.ZBot;
import com.zonerobotics.zBotAPI.ZBot.ZBotStats;
import com.zonerobotics.zBotAPI.ZMsg.ZHeader;
import com.zonerobotics.zBotAPI.ZMsg.ZImage;
import com.zonerobotics.zBotAPI.ZMsg.ZJoy;


import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;


public class SimpleBotTutorial implements KeyListener, com.zonerobotics.zBotAPI.Listeners.IBotListener{
	
	IZBot zBot = null;
	InputStream inS;
	OutputStream outS;
	public Boolean botConnected = false;
	
	// these are for the joystick tracking
    // create empty axes (x,y,z) z is camera
    float[] axes = {0.0f,0.0f,0.0f};
    // create empty buttons
    int[] buttons = {0,0,0,0,0,0,0,0,0,0};
    // seq numb for joy
    int joySeq = 0;
	
	JFrame frame = new JFrame("ZbotAPI Tut");
	JLabel JLabelImage = new JLabel();
	JLabel statusLabel = new JLabel();
	
	public void logI(String str)
	{
		System.out.println(str);
	}
	
	private boolean getStreams(String comname, int baud)
	{
		try 
		{
			CommPortIdentifier portIdentifier;
			portIdentifier = CommPortIdentifier.getPortIdentifier(comname);

			CommPort commPort = portIdentifier.open(this.getClass().getName(),2000);
			SerialPort serialPort = (SerialPort) commPort;
			serialPort.setSerialPortParams(baud,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,SerialPort.PARITY_NONE);
			
			inS = serialPort.getInputStream();
			outS = serialPort.getOutputStream();
			return true;
		} 
		catch (Exception e) 
		{
			// TODO Auto-generated catch block
			e.printStackTrace();
			// TODO FAIL EXIT
			return false;
		}		
		
	}
	
	private void setupGui()
	{
		statusLabel.setForeground(Color.white);
		statusLabel.setText("Connecting");
		Container content = frame.getContentPane();
		content.setBackground(Color.black);
		content.setLayout(new FlowLayout());
		content.add(statusLabel);
		// listen for keys to send joystick 
		frame.addKeyListener(this);
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.setMinimumSize(new Dimension(251,150));
		frame.getContentPane().add(JLabelImage, BorderLayout.CENTER);
		frame.pack();
		frame.setVisible(true);
	}
	
	public boolean startBot(String comname, int baud)
	{
		// if we cant get the stream no reason continuing
		if(!getStreams(comname, baud))
		{
			return false;
		}
		
		// setup our zbot and pass it the streams
		zBot = new ZBot( inS, outS );
		// tell it we implement its callbacks for video and events
		zBot.setBotEventListener(this);

		// setup the gui to get our data
		setupGui();

		return true;
	}
	
	public void printAllBotValues()
	{
		DecimalFormat df = new DecimalFormat("#.##");
		this.logI("=================================================");
		StringBuilder sbHeader = new StringBuilder();
		StringBuilder sbData = new StringBuilder();
		
		for ( EzReadVars rv : EzReadVars.values())
		{
			sbHeader.append(rv + "||");
			sbData.append(zBot.getValue(rv)+"||");
			//this.logI("Var: " + rv + " Value: " + zBot.getValue(rv));
		}
		this.logI(sbHeader.toString());
		this.logI(sbData.toString());
		ZBotStats zbs = zBot.getStats();
		String strStats = "TXKB: " + zbs.getTXBytes()/1000
				+ " RXKB: " + zbs.getRXBytes()/1000
				+ " TXKbps: " + df.format(zbs.getTXbps()/1024)
				+ " RXKbps: " + df.format(zbs.getRXbps()/1024);
		this.logI("Stats+ " + strStats);
		
	}
	
	public void rawToImage(byte[] rawData, int w, int h)
	{
		// make new container for the image
		BufferedImage NewImg = new BufferedImage(w, h, BufferedImage.TYPE_USHORT_565_RGB);
		// convert to short for raster
		short[] picAsShort = new short[w*h];
		ByteBuffer.wrap(rawData).order(ByteOrder.BIG_ENDIAN).asShortBuffer().get(picAsShort);
		// put directly in raster of image
		short[] imgData = ((DataBufferUShort)NewImg.getRaster().getDataBuffer()).getData();
		System.arraycopy(picAsShort, 0, imgData, 0, picAsShort.length);
		// set our jlabel to the image
		this.JLabelImage.setIcon(new ImageIcon(NewImg));
		
		// refresh
		frame.repaint();
	}


	@Override
	public void onNewImage(ZImage image) {
		if(image == null )
			return;
		// get frame number
		this.statusLabel.setText("Frame: " + image.getHeader().getSeq());
		byte[] fixedBuffer = new byte[image.getWidth()*image.getStep()];
		System.arraycopy(image.getData(), 1, fixedBuffer, 0, image.getData().length-1);
		// turn it into a image and display it
		rawToImage(fixedBuffer,image.getWidth(),image.getHeight());
	}

	@Override
	public void onNewEvent(EzEvent event) {
		switch(event)
		{
		case connected:
		{
			this.botConnected = true;
			break;
		}
		case disconnected:
		{
			this.botConnected = false;
			try {
				this.inS.close();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			try {
				this.outS.close();
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
		case imageA:
			this.onNewImage(this.zBot.getImageA());
			break;
		case imageB:
			this.onNewImage(this.zBot.getImageB());
			break;
		}
		
	}

	@Override
	public void keyTyped(KeyEvent e) {

	}

	@Override
	public void keyReleased(KeyEvent e) {
	    int keyCode = e.getKeyCode();
	    boolean sendMsg = false;
	    switch( keyCode ) { 
        case KeyEvent.VK_UP:
            axes[0] = 0f;
            sendMsg = true;
            break;
        case KeyEvent.VK_DOWN:
        	axes[0] = 0f;
        	sendMsg = true;
            break;
        case KeyEvent.VK_LEFT:
        	axes[1] = 0f;
        	sendMsg = true;
            break;
        case KeyEvent.VK_RIGHT :
        	axes[1] = 0f;
        	sendMsg = true;
            break;
        case KeyEvent.VK_PAGE_UP :
        	axes[2] = 0f;
        	sendMsg = true;
            break;
        case KeyEvent.VK_PAGE_DOWN :
        	axes[2] = 0f;
        	sendMsg = true;
            break;
	    }// end switch
	    
	    if(sendMsg)
	    {
			ZHeader hdr = new ZHeader();
			hdr.setSeq(joySeq++);
			hdr.setFrame_id("0");
			hdr.setTimestamp(new java.util.Date().getTime());
	    	ZJoy joyMsg = new ZJoy();
	    	joyMsg.setHeader(hdr);
	    	joyMsg.setAxes(axes);
	    	joyMsg.setButtons(buttons);
	    	zBot.onJoyChange(joyMsg);
	    }
		
	}

	@Override
	public void keyPressed(KeyEvent e) {
	    int keyCode = e.getKeyCode();
	    boolean sendMsg = false;
	    switch( keyCode ) { 
	        case KeyEvent.VK_UP:
	            axes[0] = 1f;sendMsg = true;
	            break;
	        case KeyEvent.VK_DOWN:
	        	axes[0] = -1f;sendMsg = true;
	            break;
	        case KeyEvent.VK_LEFT:
	        	axes[1] = -1f;sendMsg = true;
	            break;
	        case KeyEvent.VK_RIGHT :
	        	axes[1] = 1f;sendMsg = true;
	            break;
	        case KeyEvent.VK_PAGE_UP :
	        	axes[2] = 1f;sendMsg = true;
	            break;
	        case KeyEvent.VK_PAGE_DOWN :
	        	axes[2] = 1f;sendMsg = true;
	            break;
	        // TODO numbers = buttons 
	    }// end switch	
	    
	    // if they hit any of the number keys 
	    if( keyCode <= 55 && keyCode >= 49 )
	    {   // toggle the value
	    	int oldVal = this.buttons[keyCode - 49];
	    	if(oldVal == 1)
	    	{
	    		this.buttons[keyCode - 49] = 0;
	    	}else{
	    		this.buttons[keyCode - 49] = 1;
	    	}
	    	sendMsg = true;
	    }
	    
	    if(sendMsg)
	    {
			ZHeader hdr = new ZHeader();
			hdr.setSeq(joySeq++);
			hdr.setFrame_id("0");
			hdr.setTimestamp(new java.util.Date().getTime());
	    	ZJoy joyMsg = new ZJoy();
	    	joyMsg.setHeader(hdr);
	    	joyMsg.setAxes(axes);
	    	joyMsg.setButtons(buttons);
	    	zBot.onJoyChange(joyMsg);
	    }
		
	}

	@Override
	public void onLogMessage(String message) {
		System.out.println(message);
	}
}


